#ifndef __LIOMATH_H_
#define __LIOMATH_H_

#include "liobase.h"
#include "intrinsic.h"

#define MAP_U 0
#define MAP_V 1

#define AXE_X 0
#define AXE_Y 1
#define AXE_Z 2
#define AXE_W 3

#define RAD_TO_DEG 57.2957795130823208767981548141052
#define DEG_TO_RAD 0.0174532925199432957692369076848861
#define SEC_TO_MIN 0.016666666666666666666666666666667

#ifndef PI
#	define PI 3.1415926535897932384626433832795
#endif

#define INVPI 0.31830988618379067153776752674503

#ifndef EXP
#	define EXP 2.7182818284590452353602874713527
#endif

#define SQRTPI 1.7724538509055160272981674833411
#define SQRT2 1.4142135623730950488016887242097
#define SQRT3 1.7320508075688772935274463415059
#define SQRT12 3.4641016151377545870548926830117

#define MIN_VAL 0
#define MAX_VAL 1

#define EXTR_MIN -1
#define EXTR_ANY 0
#define EXTR_MAX 1

#define LOWPASS 0
#define HIGHPASS 1
#define BANDPASS 2
#define BANDSTOP 3

namespace Lio {

typedef double** MATRIX;
typedef const double*const* CMATRIX;

typedef double MATRIX3[3][3];
typedef const double CMATRIX3[3][3];

typedef double MATRIX2[2][2];
typedef const double CMATRIX2[2][2];

typedef double QUAT[4];
typedef const double CQUAT[4];

enum FIR_WIN_TYPES {
	FIR_WIN_RECT, // No window modification
	FIR_WIN_BARTLETT, // Triangular window form
	FIR_WIN_VONHANN,
	FIR_WIN_HAMMING,
	FIR_WIN_BLACKMAN,
	FIR_WIN_BLACKMAN_HARRIS_3,
	FIR_WIN_BLACKMAN_HARRIS_4,
	FIR_WIN_HARRIS_NUTALL_3,
	FIR_WIN_HARRIS_NUTALL_4,
	FIR_WIN_COUNT};

/*
#define MASK_MAX_GALOIS 0xd0000001u
#define MASK_MAX_LSFR32 0xe0000062u
#define MT_N 624
#define MT_M 397
#define MT_AMASK 0x9908b0dfUL   // constant vector a
#define MT_UMASK 0x80000000UL // most significant w-r bits
#define MT_LMASK 0x7fffffffUL // least significant r bits
#define MT_SEED 5489UL // seed by default
#define TRUNC_32 0xffffffffUL
*/

#define UNPACK3(v) v[0],v[1],v[2]
#define UNPACK4(v) v[0],v[1],v[2],v[3]

#define INVSQRTF(x) (1.0f/sqrtf((float)x))

inline float FastInvSqrt(float x)
{
	float res;
	_asm rsqrtss xmm0, x
	_asm movss res, xmm0
	return res;
}
inline float FastRecip(float x)
{
	float res;
	_asm rcpss xmm0, x
	_asm movss res, xmm0
	return res;
}
inline float FastSqrt(float x)
{
	return x ? x*FastInvSqrt(x) : 0.0f;
}
// return (-1)^power
inline int sign1(int power)
{
	return(power%2)?(-1):1;
}
// return n!
inline double fact(int n)
{
	double result=1;
	while(n>1)
		result*=n--;
	return result;
}

inline double toRad(double deg, double min = 0.0, double sec = 0.0)
{
	return DEG_TO_RAD*(deg+(min+sec*SEC_TO_MIN)*SEC_TO_MIN);
}
template <typename T>
inline T limit(const T& value,const T& min, const T& max)
{
	return (value>max)?max:(value<min)?min:value;
}
template <typename T, typename F>
inline T absRelErr(F est, T real)
{
	return T((real&&est)?100*abs((real-est)/real):0);
}
//Surface Areas:
inline double triangArea(double x1, double y1, double x2, double y2, double x3, double y3)
{
	return 0.5*absval((x1-x2)*(y1-y3)-(x1-x3)*(y1-y2));
}
inline double triangArea(double a, double b, double c)
{
	double p=0.5*(a+b+c);
	return sqrt(p*(p-a)*(p-b)*(p-c));
}
inline double angleDiff(double a1, double a2)
{
	double da=fmod(a1-a2,PI);
	if(da<-0.5*PI)
		da+=PI;
	else if(da>0.5*PI)
		da-=PI;
	return da;
}

// Random Maths
inline DWORD PseudoRandomSequence(DWORD last, DWORD max, DWORD prime = 0x797A8D77)
{
	return max?(last+prime)%max:0;
}

inline double random_u(double stdev = 1.0, double mean = 0.0)
{ // Uniform Distribution With mean Average And stdev Standart Deviation
	return mean+SQRT12*stdev*(random()-0.5);
}
// Normal Distribution, Box-Muller method (Polar method)
double random_n(double stdev = 1.0, double mean = 0.0);
double random_nv(double var = 1.0, double mean = 0.0);
void random_n(	double *n1, double *n2,	double correl = 0.0,
				double stdev1 = 1.0, double stdev2 = 1.0,
				double mean1 = 0.0, double mean2 = 0.0);
BOOL random_n3d(double *n, const double *correl = NULL, const double *stdev = NULL, const double *mean = NULL);
// Add Normal Noise To Signal
BOOL addNoise(double *data, int length, double stdev = 1.0, double mean = 0.0);
// Sample Mean (Average)
double mean(const double *values,int count);
// Sample Variance
double var(const double *values,int count);
double var(const double *values,int count,double mean);
// Allan Variance
double avar(const double *values,int count);
// Covariance
double covar(const double *values1,const double *values2,int count);
// Correlation
double correl(const double *values1,const double *values2,int count);
double conv(const double *data1, const double *data2, int count);
// Auto Correlation
double autocorrel(const double *values,int count,int shift);
double autocorrel(const double *values,int count,int shift,int index);

class Stat
{
protected:
	double x,x2,x3,x4;
	ULONG n;
public:
	double mean,var,stdev,skew,excess;
	Stat();
	Stat(const Stat& stat);
	Stat(const double *values, int count);
	void clear();
	void add(double value);
	BOOL add(const double *values, int count);
	BOOL update();
	DWORD length() const {return n;}
};

class ComplexNumber
{
public:
	double real,imag;
	ComplexNumber():real(0),imag(0){}
	ComplexNumber(double r, double i):real(r),imag(i){}
	ComplexNumber(const ComplexNumber &c):real(c.real),imag(c.imag){}
	void copy(const ComplexNumber &c)
	{
		real=c.real;
		imag=c.imag;
	}
	ComplexNumber& operator =(const ComplexNumber &c)
	{
		copy(c);
		return *this;
	}
	BOOL equals(const ComplexNumber &c)
	{
		return real==c.real&&imag==c.imag;
	}
	BOOL operator ==(const ComplexNumber &c)
	{
		return equals(c);
	}
	void setReIm(double r,double i)
	{
		real=r;
		imag=i;
	}
	void setModAng(double m,double a)
	{
		real=m*cos(a);
		imag=m*sin(a);
	}
	ComplexNumber& operator +=(const ComplexNumber &c)
	{
		real+=c.real;
		imag+=c.imag;
		return *this;
	}
	ComplexNumber& operator +=(double n)
	{
		real+=n;
		return *this;
	}
	ComplexNumber& operator -=(const ComplexNumber &c)
	{
		real-=c.real;
		imag-=c.imag;
		return *this;
	}
	ComplexNumber& operator -=(double n)
	{
		real-=n;
		return *this;
	}
	ComplexNumber& operator *=(const ComplexNumber &c)
	{
		real=real*c.real-imag*c.imag;
		imag=real*c.imag+imag*c.real;
		return *this;
	}
	ComplexNumber& operator *=(double n)
	{
		real*=n;
		imag*=n;
		return *this;
	}
	ComplexNumber& operator /=(const ComplexNumber &c)
	{
		double tmp=INV(c.real*c.real+c.imag*c.imag);
		real=(real*c.real+imag*c.imag)*tmp;
		imag=(-real*c.imag+imag*c.real)*tmp;
		return *this;
	}
	ComplexNumber& operator /=(double n)
	{
		real/=n;
		imag/=n;
		return *this;
	}
	friend ComplexNumber operator +(const ComplexNumber &c1, const ComplexNumber &c2)
	{
		ComplexNumber c(c1.real+c2.real,c1.imag+c2.imag);
		return c;
	}
	friend ComplexNumber operator +(const ComplexNumber &c1, double n)
	{
		ComplexNumber c(c1.real+n,c1.imag);
		return c;
	}
	friend ComplexNumber operator +(double n, const ComplexNumber &c2)
	{
		ComplexNumber c(n+c2.real,c2.imag);
		return c;
	}
	friend ComplexNumber operator -(const ComplexNumber &c1, const ComplexNumber &c2)
	{
		ComplexNumber c(c1.real-c2.real,c1.imag-c2.imag);
		return c;
	}
	friend ComplexNumber operator -(const ComplexNumber &c1, double n)
	{
		ComplexNumber c(c1.real-n,c1.imag);
		return c;
	}
	friend ComplexNumber operator -(double n, const ComplexNumber &c2)
	{
		ComplexNumber c(n-c2.real,-c2.imag);
		return c;
	}
	friend ComplexNumber operator *(const ComplexNumber &c1, const ComplexNumber &c2)
	{
		ComplexNumber c(
			c1.real*c2.real-c1.imag*c2.imag,
			c1.real*c2.imag+c1.imag*c2.real);
		return c;
	}
	friend ComplexNumber operator *(const ComplexNumber &c1, double n)
	{
		ComplexNumber c(c1.real*n,c1.imag*n);
		return c;
	}
	friend ComplexNumber operator *(double n, const ComplexNumber &c2)
	{
		ComplexNumber c(n*c2.real,n*c2.imag);
		return c;
	}
	friend ComplexNumber operator /(const ComplexNumber &c1, const ComplexNumber &c2)
	{
		double tmp=INV(c2.real*c2.real+c2.imag*c2.imag);
		ComplexNumber c(
			(c1.real*c2.real+c1.imag*c2.imag)*tmp,
			(-c1.real*c2.imag+c1.imag*c2.real)*tmp);
		return c;
	}
	friend ComplexNumber operator /(const ComplexNumber &c1, double n)
	{
		ComplexNumber c(c1.real/n,c1.imag/n);
		return c;
	}
	friend ComplexNumber operator /(double n, const ComplexNumber &c2)
	{
		double tmp=INV(c2.real*c2.real+c2.imag*c2.imag);
		ComplexNumber c(
			(n*c2.real)*tmp,
			(-n*c2.imag)*tmp);
		return c;
	}
	double arg() const
	{
		return atan2(imag,real);
	}
	double mod() const
	{
		return sqrt(real*real+imag*imag);
	}
	double mod2() const
	{
		return real*real+imag*imag;
	}
};


// Matrix math
BOOL importMatrix(	MATRIX m, int rows, int cols,
					const double *data,
					BOOL colrow = FALSE);
BOOL exportMatrix(	CMATRIX m, int rows, int cols,
					double *data,
					BOOL colrow = FALSE);

BOOL randMatrix(MATRIX m, int rows, int cols, double min = 0.0, double max = 1.0);
void randMatrix(MATRIX3 m, double min = 0.0, double max = 1.0);
void randMatrix(MATRIX2 m, double min = 0.0, double max = 1.0);

BOOL copyMatrix(CMATRIX m, MATRIX out, int rows, int cols);

BOOL oneMatrix(MATRIX m, int rows, int cols);
void oneMatrix(MATRIX3 m);

void clearMatrix(MATRIX m, int rows, int cols);

BOOL mulMatrix(CMATRIX m1, CMATRIX m2, MATRIX out, int rows1, int cols1_rows2, int cols2, double coef);
BOOL mulMatrix(CMATRIX m1, CMATRIX m2, MATRIX out, int rows1, int cols1_rows2, int cols2);
BOOL mulMatrix(CMATRIX m, double n, MATRIX out, int rows, int cols);
void mulMatrix(CMATRIX3 m1, MATRIX3 m2, MATRIX3 out = NULL);
void mulMatrix(CMATRIX2 m1, MATRIX2 m2, MATRIX2 out = NULL);
void mulMatrix(MATRIX3 m, double n, MATRIX3 out);

BOOL mulMatrixTrans(CMATRIX m1, CMATRIX m2, MATRIX out,	int rows1, int cols1);
BOOL mulMatrixTrans(const double  m1[2][2], CMATRIX2 m2, MATRIX2 out);

BOOL addMatrix(CMATRIX m1, CMATRIX m2, MATRIX out, int rows, int cols);
BOOL addMatrix(CMATRIX m1, CMATRIX m2, MATRIX out, int rows, int cols, double k1, double k2);
void addMatrix(CMATRIX3 m1, MATRIX3 m2, MATRIX3 out = NULL);
void addMatrix(CMATRIX3 m1, MATRIX3 m2, double k1, double k2, MATRIX3 out = NULL);
void addMatrix(CMATRIX2 m1, MATRIX2 m2, MATRIX2 out = NULL);

double detMatrix(CMATRIX m, int dim, double eps = 0.0, MATRIX buffer = NULL);
double detMatrix(CMATRIX3 m);
double detMatrix(CMATRIX2 m);

BOOL invMatrix(CMATRIX m, MATRIX out, int dim, double eps = 0.0, MATRIX buffer = NULL);
BOOL invMatrix(MATRIX3 m, MATRIX3 out = NULL);
BOOL invMatrix(MATRIX2 m, MATRIX2 out = NULL);

BOOL transMatrix(CMATRIX m, MATRIX out, int rows, int cols);
void transMatrix(MATRIX3 m, MATRIX3 out = NULL);
void transMatrix(MATRIX2 m, MATRIX2 out = NULL);

double traceMatrix(CMATRIX m, int rows, int cols);
double traceMatrix(CMATRIX3 m);

void printMatrix(FILE *file, CMATRIX m, int rows, int cols, CSTR start = NULL, CSTR line = NULL, CSTR format = NULL, CSTR end = NULL);
void printMatrix(FILE *file, CMATRIX3 m, CSTR start = NULL, CSTR line = NULL, CSTR format = NULL, CSTR end = NULL);
void printMatrix(FILE *file, CMATRIX2 m, CSTR start = NULL, CSTR line = NULL, CSTR format = NULL, CSTR end = NULL);

void importMatrix(MATRIX3 m, const double data[9], BOOL colrow = FALSE);
void exportMatrix(CMATRIX3 m, double data[9], BOOL colrow = FALSE);


template <int rows, int cols = rows, class T = double>
struct M
{
	static const M null;
	T elem[rows * cols];
	M<cols, rows, T> trans() const
	{
		M<cols, rows, T> t;
		for(int row=0,i=0;row<rows;row++)
			for(int col=0;col<cols;col++,i++)
				t.elem[col*rows + row] = elem[i];
		return t;
	}
	int Rows() const { return rows; }
	int Cols() const { return cols; }
	void print() const
	{
		PRINT("\n");
		for(int row=0;row<rows;row++)
		{
			for(int col=0;col<cols;col++)
				PRINT("%f ", elem[row*cols + col]);
			PRINT("\n");
		}
	}
	T det() const
	{
		if( rows != cols || cols <= 0 )
			return 0;
		switch(cols)
		{
			case 1: //1x1
				return
						elem[0];
			case 2: //2x2
				return
					+elem[0] * elem[3]
					-elem[1] * elem[2];
			case 3: //3x3
				return 
						elem[0] * (elem[4] * elem[8] - elem[5] * elem[7])
    				-elem[3] * (elem[8] * elem[1] - elem[7] * elem[2])
					+elem[6] * (elem[1] * elem[5] - elem[2] * elem[4]);
			case 4: // 4x4
				return
					elem[0] * (
						elem[5]  *  (elem[10] * elem[15] - elem[11] * elem[14])
						-elem[9] *  (elem[6] * elem[15]  - elem[7] * elem[14])
						+elem[13] * (elem[11] * elem[6]  - elem[10] * elem[7]))
					-elem[4] * (
						elem[1]  *  (elem[10] * elem[15] - elem[11] * elem[14])
						-elem[9] *  (elem[2] * elem[15]  - elem[3] * elem[14])
						+elem[13] * (elem[2] * elem[11]  - elem[3] * elem[10]))
					+elem[8] * (
						elem[1]  *  (elem[6] * elem[15] - elem[7] * elem[14])
						-elem[5] *  (elem[2] * elem[15] - elem[3] * elem[14])
						+elem[13] * (elem[2] * elem[7]  - elem[3] * elem[6]))
					-elem[12] * (
						elem[1]  *  (elem[6] * elem[11] - elem[7] * elem[10])
						-elem[5] *  (elem[2] * elem[11] - elem[3] * elem[10])
						+elem[9]  * (elem[2] * elem[7]  - elem[3] * elem[6]));
		}
		M tmpm = *this;
		register int k, col, row;
		int y1, y2;
		T d,*tmp = tmpm.elem;
		d = elem[0];
		for(k = 0, y1 = 0; k < cols-1; k++, y1 += cols)
		{
			if(!tmp[y1 + k])
			{
				BOOL degen = TRUE;
				for (row = k+1, y2 = row*cols; row<cols; row++, y2 += cols)
				{
					if(tmp[y2 + k])
					{
						degen = FALSE;
						// Add Row 'row' To Row 'k'
						for(col = 0; col < cols; col++)
							tmp[y1 + col] += tmp[y2 + col];
						break;
					}
				}
				if(degen)
					return 0;
			}
			T coef1 = -1 / tmp[y1 + k];
			for (row = k+1, y2 = row*cols; row < cols; row++, y2 += cols)
			{
				T coef2 = coef1 * tmp[y2 + k];
				//Add Row 'k' Multiplied By 'coef' To Row 'row'
				for(col = k; col < cols; col++)
					tmp[y2 + col] += coef2 * tmp[y1 + col];
			}
			d *= tmp[y1 + cols + k + 1];
		}
		return d;
	}
	M<cols, cols, T> inv() const
	{
		if( rows != cols || cols <= 0 )
			return null;
		switch(cols)
		{
			case 1: // 1x1
			{
				if( !elem[0] )
					return null;
				M<cols, cols, T> out = {1/elem[0]};
				return out;
			}
			case 2: // 2x2
			{
				T det = elem[0] * elem[3] - elem[1] * elem[2];
				if( !det )
					return null;
				T coef = 1/det;
				M<cols, cols, T> out = {
					elem[3] * coef, -elem[1] * coef,
					-elem[2] * coef,  elem[0] * coef
				};
				return out;
			}
			case 3: // 3x3
			{
				T inv[9];

				inv[0] = elem[8] * elem[4] - elem[7] * elem[5];
				inv[1] = elem[7] * elem[2] - elem[8] * elem[1];
				inv[2] = elem[5] * elem[1] - elem[4] * elem[2];

				T det = elem[0] * inv[0] + elem[3] * inv[1] + elem[6] * inv[2];
				if (!det)
					return null;

				inv[3] = elem[6] * elem[5] - elem[8] * elem[3];
				inv[4] = elem[8] * elem[0] - elem[6] * elem[2];
				inv[5] = elem[3] * elem[2] - elem[5] * elem[0];

				inv[6] = elem[7] * elem[3] - elem[6] * elem[4];
				inv[7] = elem[6] * elem[1] - elem[7] * elem[0];
				inv[8] = elem[4] * elem[0] - elem[3] * elem[1];

				T coef = 1.0 / det;
				M<cols, cols, T> out;
				for (int i = 0; i < 9; i++)
					out.elem[i] = inv[i] * coef;
				return out;
			}
			case 4: // 4x4
			{
				T inv[16];

				inv[0] =    elem[5]  * elem[10] * elem[15] - 
							elem[5]  * elem[11] * elem[14] - 
							elem[9]  * elem[6]  * elem[15] + 
							elem[9]  * elem[7]  * elem[14] +
							elem[13] * elem[6]  * elem[11] - 
							elem[13] * elem[7]  * elem[10];

				inv[4] =   -elem[4]  * elem[10] * elem[15] + 
							elem[4]  * elem[11] * elem[14] + 
							elem[8]  * elem[6]  * elem[15] - 
							elem[8]  * elem[7]  * elem[14] - 
							elem[12] * elem[6]  * elem[11] + 
							elem[12] * elem[7]  * elem[10];

				inv[8] =    elem[4]  * elem[9]  * elem[15] - 
							elem[4]  * elem[11] * elem[13] - 
							elem[8]  * elem[5]  * elem[15] + 
							elem[8]  * elem[7]  * elem[13] + 
							elem[12] * elem[5]  * elem[11] - 
							elem[12] * elem[7]  * elem[9];

				inv[12] =  -elem[4]  * elem[9]  * elem[14] + 
							elem[4]  * elem[10] * elem[13] +
							elem[8]  * elem[5]  * elem[14] - 
							elem[8]  * elem[6]  * elem[13] - 
							elem[12] * elem[5]  * elem[10] + 
							elem[12] * elem[6]  * elem[9];

				T det = elem[0] * inv[0] + elem[1] * inv[4] + elem[2] * inv[8] + elem[3] * inv[12];
				if (!det)
					return null;

				inv[1] =   -elem[1]  * elem[10] * elem[15] + 
							elem[1]  * elem[11] * elem[14] + 
							elem[9]  * elem[2] * elem[15] - 
							elem[9]  * elem[3] * elem[14] - 
							elem[13] * elem[2] * elem[11] + 
							elem[13] * elem[3] * elem[10];

				inv[5] =    elem[0]  * elem[10] * elem[15] - 
							elem[0]  * elem[11] * elem[14] - 
							elem[8]  * elem[2] * elem[15] + 
							elem[8]  * elem[3] * elem[14] + 
							elem[12] * elem[2] * elem[11] - 
							elem[12] * elem[3] * elem[10];

				inv[9] =   -elem[0]  * elem[9] * elem[15] + 
							elem[0]  * elem[11] * elem[13] + 
							elem[8]  * elem[1] * elem[15] - 
							elem[8]  * elem[3] * elem[13] - 
							elem[12] * elem[1] * elem[11] + 
							elem[12] * elem[3] * elem[9];

				inv[13] =   elem[0]  * elem[9] * elem[14] - 
							elem[0]  * elem[10] * elem[13] - 
							elem[8]  * elem[1] * elem[14] + 
							elem[8]  * elem[2] * elem[13] + 
							elem[12] * elem[1] * elem[10] - 
							elem[12] * elem[2] * elem[9];

				inv[2] =    elem[1]  * elem[6] * elem[15] - 
							elem[1]  * elem[7] * elem[14] - 
							elem[5]  * elem[2] * elem[15] + 
							elem[5]  * elem[3] * elem[14] + 
							elem[13] * elem[2] * elem[7] - 
							elem[13] * elem[3] * elem[6];

				inv[6] =   -elem[0]  * elem[6] * elem[15] + 
							elem[0]  * elem[7] * elem[14] + 
							elem[4]  * elem[2] * elem[15] - 
							elem[4]  * elem[3] * elem[14] - 
							elem[12] * elem[2] * elem[7] + 
							elem[12] * elem[3] * elem[6];

				inv[10] =   elem[0]  * elem[5] * elem[15] - 
							elem[0]  * elem[7] * elem[13] - 
							elem[4]  * elem[1] * elem[15] + 
							elem[4]  * elem[3] * elem[13] + 
							elem[12] * elem[1] * elem[7] - 
							elem[12] * elem[3] * elem[5];

				inv[14] =  -elem[0]  * elem[5] * elem[14] + 
							elem[0]  * elem[6] * elem[13] + 
							elem[4]  * elem[1] * elem[14] - 
							elem[4]  * elem[2] * elem[13] - 
							elem[12] * elem[1] * elem[6] + 
							elem[12] * elem[2] * elem[5];

				inv[3] =   -elem[1] * elem[6] * elem[11] + 
							elem[1] * elem[7] * elem[10] + 
							elem[5] * elem[2] * elem[11] - 
							elem[5] * elem[3] * elem[10] - 
							elem[9] * elem[2] * elem[7] + 
							elem[9] * elem[3] * elem[6];

				inv[7] =    elem[0] * elem[6] * elem[11] - 
							elem[0] * elem[7] * elem[10] - 
							elem[4] * elem[2] * elem[11] + 
							elem[4] * elem[3] * elem[10] + 
							elem[8] * elem[2] * elem[7] - 
							elem[8] * elem[3] * elem[6];

				inv[11] =  -elem[0] * elem[5] * elem[11] + 
							elem[0] * elem[7] * elem[9] + 
							elem[4] * elem[1] * elem[11] - 
							elem[4] * elem[3] * elem[9] - 
							elem[8] * elem[1] * elem[7] + 
							elem[8] * elem[3] * elem[5];

				inv[15] =   elem[0] * elem[5] * elem[10] - 
							elem[0] * elem[6] * elem[9] - 
							elem[4] * elem[1] * elem[10] + 
							elem[4] * elem[2] * elem[9] + 
							elem[8] * elem[1] * elem[6] - 
							elem[8] * elem[2] * elem[5];

				T coef = 1.0 / det;
				M<cols, cols, T> out;
				for (int i = 0; i < 16; i++)
					out.elem[i] = inv[i] * coef;
				return out;
			}
		}
		M<cols, cols, T> outm = {0}, tmpm = *this;
		T coef, *tmp = tmpm.elem, *out = outm.elem;
		for(int i=0;i<cols;i++)
			out[i*cols+i]=1;
		register int row,col,k;
		int y1, y2;
		for (row = 0, y1 = row * cols; row < cols-1; row++, y1 += cols)
		{
			if(!tmp[y1 + row])
			{
				BOOL degen=TRUE;
				for (k = row+1, y2 = k * cols; k<cols; k++, y2 += cols)
				{
					if(tmp[y2 + row])
					{
						degen=FALSE;
						for (col = 0; col<cols; col++)
						{
							tmp[y1 + col] += tmp[y2 + col];
							out[y1 + col] += out[y2 + col];
						}
						break;
					}
				}
				if(degen)
					return null;
			}
			coef = 1 / tmp[y1 + row];
			for (col = 0; col<cols; col++)
			{
				tmp[y1 + col] *= coef;
				out[y1 + col] *= coef;
			}
			for (k = row+1, y2 = k * cols; k<cols; k++, y2 += cols)
			{
				coef = -tmp[y2 + row];
				for (col = 0; col<cols; col++)
				{
					tmp[y2 + col] += coef * tmp[y1 + col];
					out[y2 + col] += coef * out[y1 + col];
				}
			}
		}
		int y = (cols-1) * cols;
		if(!tmp[y + cols-1])
			return null;
		coef = 1 / tmp[y + cols-1];
		for (col = 0; col<cols; col++)
		{
			tmp[y + col] *= coef;
			out[y + col] *= coef;
		}
		for (row = cols-1, y1 = row*cols; row>0; row--, y1 -= cols)
		{
			for (k = row-1, y2 = k*cols; k>=0; k--, y2 -= cols)
			{
				coef = -tmp[y2 + row];
				for (col = 0; col<cols; col++)
					out[y2 + col] += coef * out[y1 + col];
			}
		}
		return outm;
	}
	M<cols, rows, T> pseudoInv() const
	{
		auto mt = trans();
		return (mt * (*this)).inv() * mt;
	}
	T trace() const
	{
		T result = 0;
		for(int i=0;i<rows&&i<cols;i++)
			result += elem[i*cols + i];
		return result;    
	}
	T& operator[] (int i) { return elem[i]; }
	const T& operator[] (int i) const { return elem[i]; }
	T& operator() (int row, int col) { return elem[row*cols + col]; }
	const T& operator() (int row, int col) const { return elem[row*cols + col]; }
	BOOL operator == (const M<rows, cols, T> &m) const
	{
		return memcmp(elem, m.elem, sizeof(*elem)) == 0;
	}
	BOOL operator != (const M<rows, cols, T> &m) const
	{
		return memcmp(elem, m.elem, sizeof(*elem)) != 0;
	}
	void clear()
	{
		memset(elem, 0, sizeof(*elem));
	}
	void rand()
	{
		for(int i=0;i<rows*cols;i++)
			elem[i] = (T)random();
	}
	void one()
	{
		for(int row = 0, y = 0; row < rows; row++, y += cols)
			for(int col = 0; col < cols; col++)
				elem[y + col] = row == col;
	}
};

template <int rows, int cols, class T>
const M<rows, cols, T> M<rows, cols, T>::null = {0};

template <int rows1, int cols1_rows2, int cols2, class T>
inline M<rows1, cols2, T> operator * (const M<rows1, cols1_rows2, T> &m1, const M<cols1_rows2, cols2, T> &m2)
{
	M<rows1, cols2, T> m;
	register int row, col, k;
	int y, y1;
	for(row = 0, y = 0, y1 = 0; row < rows1; row++, y += cols2, y1 += cols1_rows2)
	{
		for(col = 0; col < cols2; col++)
		{
			T tmp=0;
			for(k = 0; k < cols1_rows2; k++)
				tmp += m1.elem[y1 + k] * m2.elem[k*cols2 + col];
			m.elem[y + col] = tmp;
		}
	}
	return m;
}
template <int rows, int cols, class T>
inline M<rows, cols, T> operator * (const M<rows, cols, T> &m, T n)
{
	M<rows, cols, T> out;
	for(register int i=0; i < rows * cols; i++)
		out.elem[i] = m.elem[i] * n;
	return out;
}

template <int rows, int cols, class T>
inline M<rows, cols, T> operator * (T n, const M<rows, cols, T> &m)
{
	M<rows, cols, T> out;
	for(register int i=0; i < rows * cols; i++)
		out.elem[i] = m.elem[i] * n;
	return out;
}
template <int rows, int cols, class T>
inline M<rows, cols, T> operator + (const M<rows, cols, T> &m1, const M<rows, cols, T> &m2)
{
	M<rows, cols, T> out;
	for(register int i=0; i < rows * cols; i++)
		out.elem[i] = m1.elem[i] + m2.elem[i];
	return out;
}
template <int rows, int cols, class T>
inline M<rows, cols, T> operator - (const M<rows, cols, T> &m1, const M<rows, cols, T> &m2)
{
	M<rows, cols, T> out;
	for(register int i=0; i < rows * cols; i++)
		out.elem[i] = m1.elem[i] - m2.elem[i];
	return out;
}


class Matrix:public Table<double>
{
public:
	Matrix()
	{}
	Matrix::Matrix(int rows,int cols, BOOL clear = TRUE)
	{
		if(!setSize(rows,cols,clear)) throw MEM_ERROR;
	}
	Matrix::Matrix(const Matrix &m)
	{
		if(!copy(m)) throw MEM_ERROR;
	}
	BOOL diag(double *diagElems);
	Matrix diag() const;
	void one(){oneMatrix(elems,rows,cols);}
	void clear(){clearMatrix(elems,rows,cols);}
	void rand(double min = 0.0, double max = 1.0){randMatrix(elems,rows,cols,min,max);}
	Matrix trans() const;
	Matrix inv(double eps = 0.0, BOOL pseudo = TRUE) const;
	double det(double eps = 0.0) const {return (rows==cols)?detMatrix(elems,rows,eps):0;}
	double trace()const{return traceMatrix(elems,rows,cols);}
	BOOL import(CSTR str, CSTR colSep = _T(" "), CSTR rowSep = _T(";"));
	void print(FILE *file = stdout, CSTR start = NULL, CSTR line = NULL, CSTR format = NULL, CSTR end = NULL)
	{
		printMatrix(file,elems,rows,cols,start,line,format,end);
	}
	// Operations
	operator BOOL () {return rows==0||cols==0;}
	operator double () {return det();}
	Matrix & operator = (const Matrix &m)
	{
		if(!copy(m))
			throw MEM_ERROR;
		return *this;
	}
	BOOL operator == (const Matrix &m) const
	{
		return equals(m);
	}
	Matrix & operator *= (double n)
	{
		mulMatrix(elems,n,elems,rows,cols);
		return *this;
	}
	Matrix & operator += (const Matrix &m)
	{
		if(cols!=m.cols||rows!=m.rows)
			throw DIM_ERROR;
		addMatrix(m.elems,elems,elems,rows,cols);
		return *this;
	}
	Matrix & operator -= (const Matrix &m)
	{
		if(cols!=m.cols||rows!=m.rows)
			throw DIM_ERROR;
		addMatrix(m.elems,elems,elems,rows,cols,-1,1);
		return *this;
	}
	friend Matrix operator * (const Matrix &m1,const Matrix &m2)
	{
		if(m1.cols!=m2.rows)
			throw DIM_ERROR;
		Matrix m(m1.rows,m2.cols);
		mulMatrix(m1.elems,m2.elems,m.elems,m1.rows,m1.cols,m2.cols);
		return m;
	}
	friend Matrix operator * (const Matrix &m, double n)
	{
		Matrix out(m.rows,m.cols);
		mulMatrix(m.elems,n,out.elems,m.rows,m.cols);
		return out;
	}
	friend Matrix operator * (double n,const Matrix &m)
	{
		Matrix out(m.rows,m.cols);
		mulMatrix(m.elems,n,out.elems,m.rows,m.cols);
		return out;
	}
	friend Matrix operator + (const Matrix &m1,const Matrix &m2)
	{
		if(m1.cols!=m2.cols||m1.rows!=m2.rows)
			throw DIM_ERROR;
		Matrix out(m1.rows,m1.cols);
		addMatrix(m1.elems,m2.elems,out.elems,out.rows,out.cols);
		return out;
	}
	friend Matrix operator - (const Matrix &m1,const Matrix &m2)
	{
		if(m1.cols!=m2.cols||m1.rows!=m2.rows)
			throw DIM_ERROR;
		Matrix out(m1.rows,m1.cols);
		addMatrix(m1.elems,m2.elems,out.elems,out.rows,out.cols,1,-1);
		return out;
	}
};
class MatrixInvertor
{
protected:
	Matrix mInvBuff;
	MATRIX invBuff;
	int rows,cols;
public:
	Matrix mMt,mMtM,mInvMtM,mInvM;
	CMATRIX M;
	MATRIX Mt,MtM,invMtM,invM;
	MatrixInvertor():M(NULL),rows(0),cols(0){}
	MATRIX bind(const Matrix *M)
	{
		return M?bind(M->elems,M->rows,M->cols):NULL;
	}
	MATRIX bind(CMATRIX M, int rows, int cols);
	MATRIX inv(double eps = 0.0);
};
// Quaternion math
void initQuat(QUAT q, double w = 1.0, double x = 0.0, double y = 0.0, double z = 0.0);
void initQuat(QUAT q, CQUAT q0 = NULL);
void mulQuat(CQUAT q1, QUAT q2, QUAT out = NULL);
void mulQuat(QUAT q, double n, QUAT out = NULL);
void addQuat(QUAT q1, QUAT q2, QUAT out = NULL);
void conjQuat(QUAT q, QUAT out = NULL);
void invQuat(QUAT q, QUAT out = NULL);
void normQuat(QUAT q, QUAT out = NULL);
double getQuatNorm(QUAT q);
double getQuatUnit(QUAT q, double unit[3]);
void printQuat(FILE *file, QUAT q, STR format = NULL, STR start = NULL, STR end = NULL);

// Vector math
template <typename T>
inline double rotVector(CQUAT q, T v[3], T out[3] = NULL)
{
	if(!out)out=v;
	QUAT tmp;
	// v*q_conj
	tmp[0]= v[0]*q[1]+v[1]*q[2]+v[2]*q[3];
	tmp[1]= v[0]*q[0]-v[1]*q[3]+v[2]*q[2];
	tmp[2]= v[1]*q[0]-v[2]*q[1]+v[0]*q[3];
	tmp[3]= v[2]*q[0]-v[0]*q[2]+v[1]*q[1];
	mulQuat(q,tmp);
	out[0]=(T)tmp[1];
	out[1]=(T)tmp[2];
	out[2]=(T)tmp[3];
	return (T)tmp[0];
}
template <typename T, typename F>
inline void skew(const T v[3], F out[3][3])
{
	out[0][0]=F(0);
	out[0][1]=F(-v[2]);
	out[0][2]=F(v[1]);

	out[1][0]=F(v[2]);
	out[1][1]=F(0);
	out[1][2]=F(-v[0]);

	out[2][0]=F(-v[1]);
	out[2][1]=F(v[0]);
	out[2][2]=F(0);
}
template <typename T>
inline void initVector(T v[3], const T v0[3] = NULL)
{
	if(v0)
	{
		v[0]=v0[0];
		v[1]=v0[1];
		v[2]=v0[2];
	}
	else
	{
		v[0]=v[1]=v[2]=0;
	}
}
template <typename T, typename A, typename B, typename C>
inline void initVector(T v[3], A x, B y, C z)
{
	v[0]=T(x);
	v[1]=T(y);
	v[2]=T(z);
}
template <typename T, typename A, typename B>
inline void randVector(T v[3], A min, B max)
{
	v[0]=(T)random(min,max);
	v[1]=(T)random(min,max);
	v[2]=(T)random(min,max);
}
template <typename T>
inline void mulVector(T v[3], double n, T out[3] = NULL)
{
	if(!out)out=v;
	out[0]=T(v[0]*n);
	out[1]=T(v[1]*n);
	out[2]=T(v[2]*n);
}
template <typename T, typename F>
inline void mulVector(const F m[3][3], T v[3], T out[3] = NULL)
{
	T tmp[3];
	if(!out)out=v;
	tmp[0]=T(m[0][0]*v[0]+m[0][1]*v[1]+m[0][2]*v[2]);
	tmp[1]=T(m[1][0]*v[0]+m[1][1]*v[1]+m[1][2]*v[2]);
	tmp[2]=T(m[2][0]*v[0]+m[2][1]*v[1]+m[2][2]*v[2]);
	out[0]=tmp[0];
	out[1]=tmp[1];
	out[2]=tmp[2];
}
template <typename T, typename F>
inline void mulVector(const F m[2][2], T v[2], T out[2] = NULL)
{
	T tmp[2];
	if(!out)out=v;
	tmp[0]=T(m[0][0]*v[0]+m[0][1]*v[1]);
	tmp[1]=T(m[1][0]*v[0]+m[1][1]*v[1]);
	out[0]=tmp[0];
	out[1]=tmp[1];
}
template <typename T>
void limitVector(T v[3], T min, T max, T out[3] = NULL)
{
	if(!out)out=v;
	out[0]=limit(v[0],min,max);
	out[1]=limit(v[1],min,max);
	out[2]=limit(v[2],min,max);
}
// out = k1*v1+k2*v2
template <typename T, typename F>
inline void addVector(const F v1[3], T v2[3], double k1, double k2, T out[3] = NULL)
{
	if(!out)out=v2;
	out[0]=T(k1*v1[0]+k2*v2[0]);
	out[1]=T(k1*v1[1]+k2*v2[1]);
	out[2]=T(k1*v1[2]+k2*v2[2]);
}
// out = v1+v2
template <typename T, typename F>
inline void addVector(const F v1[3], T v2[3], T out[3] = NULL)
{
	if(!out)out=v2;
	out[0]=T(v1[0]+v2[0]);
	out[1]=T(v1[1]+v2[1]);
	out[2]=T(v1[2]+v2[2]);
}
template <typename T>
inline void addVector(T v[3], double n, T out[3] = NULL)
{
	if(!out)out=v;
	out[0]=T(v[0]+n);
	out[1]=T(v[1]+n);
	out[2]=T(v[2]+n);
}
//out=k*v1+(1-k)*v2
template <typename T, typename F>
inline void mixVector(const F v1[3], T v2[3], double k, T out[3] = NULL)
{
	if(!out)out=v2;
	//if(k>1)k=1;
	//else if(k<0)k=0;
	out[0]=T(k*v1[0]+(1-k)*v2[0]);
	out[1]=T(k*v1[1]+(1-k)*v2[1]);
	out[2]=T(k*v1[2]+(1-k)*v2[2]);
}
// Create Vector From Starting And Ending Point2d:
template <typename T>
inline void getVector(const T vStart[3], const T vEnd[3], T v[3])
{
	v[0]=vEnd[0]-vStart[0];
	v[1]=vEnd[1]-vStart[1];
	v[2]=vEnd[2]-vStart[2];
}
template <typename T, typename F, typename L>
inline void cross(const T v1[3], const F v2[3], L product[3])
{
	product[0]=(L)(v1[1]*v2[2]-v1[2]*v2[1]);
	product[1]=(L)(v1[2]*v2[0]-v1[0]*v2[2]);
	product[2]=(L)(v1[0]*v2[1]-v1[1]*v2[0]);
}
template <typename T, typename F, typename L>
inline void cross(const F v1[3], const T v2[3], L product[3], double k1, double k2)
{
	product[0]=(L)(k1*k2*(v1[1]*v2[2]-v1[2]*v2[1]));
	product[1]=(L)(k1*k2*(v1[2]*v2[0]-v1[0]*v2[2]));
	product[2]=(L)(k1*k2*(v1[0]*v2[1]-v1[1]*v2[0]));
}
template <typename T, typename F>
inline T dot(const F v1[3], const T v2[3])
{
	return T(v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]);
}
template <typename T>
inline T getSquare(T x, T y, T z)
{
	return x*x+y*y+z*z;
}
template <typename T>
inline T getSquare(T x, T y)
{
	return x*x+y*y;
}
inline double getLength(const double v[3])
{
	return sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
}
inline float getLength(const float v[3])
{
	return sqrtf(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
}
inline double getLength(double x, double y)
{
	return sqrt(x*x+y*y);
}
inline float getLength(float x, float y)
{
	return sqrtf(x*x+y*y);
}
inline double getLength(double x, double y, double z)
{
	return sqrt(x*x+y*y+z*z);
}
inline float getLength(float x, float y, float z)
{
	return sqrtf(x*x+y*y+z*z);
}
inline float getVectorNorm(float x, float y)
{
	return (x||y)?FastInvSqrt(x*x+y*y):0;
}
inline float getVectorNorm(float x, float y, float z)
{
	return (x||y||z)?FastInvSqrt(x*x+y*y+z*z):0;
}
inline float getVectorNorm(float v[3])
{
	return getVectorNorm(v[0],v[1],v[2]);
}
inline BOOL getUnitVector(float v[3], float out[3] = NULL)
{
	float den=getVectorNorm(v);
	if(!den)return FALSE;
	if(!out)out=v;
	out[0]=den*v[0];
	out[1]=den*v[1];
	out[2]=den*v[2];
	return TRUE;
}
// Calculate the APPROX normal "normal" in vertex "v1", where "v2" and "v3" 
// are two successive vertice of the same plane:
inline BOOL getNormalVector(const float v1[3], const float v2[3], const float v3[3], float n[3])
{
	n[0]=(v3[1]-v1[1])*(v2[2]-v1[2])-(v3[2]-v1[2])*(v2[1]-v1[1]);
	n[1]=(v3[2]-v1[2])*(v2[0]-v1[0])-(v3[0]-v1[0])*(v2[2]-v1[2]);
	n[2]=(v3[0]-v1[0])*(v2[1]-v1[1])-(v3[1]-v1[1])*(v2[0]-v1[0]);
	return getUnitVector(n);
}


template <typename T>
inline void printVector(FILE *file, const T v[3], STR start = NULL, STR end = NULL, STR format = NULL)
{
	int i;
	if(format==NULL)
		format=_T("%f ");
	if(start)
		_ftprintf(file,start);
	for(i=0;i<3;i++)
		_ftprintf(file,format,v[i]);
	if(end)
		_ftprintf(file,end);
}

// Modeling math
// Bezier method: Pseudo-Interpolation between 4 points
// out = P1*t^3+P2*3*t^2*(1-t)+P3*3*t*(1-t)^2+P3*(1-t)^3
template <typename T, typename F>
inline void bezier(	F t, // from 0 to 1
					T P[4][3], // 4 points
					T out[3]) // resulting point
{
	F a[4]={t*t*t,3*t*t*(1-t),3*t*(1-t)*(1-t),(1-t)*(1-t)*(1-t)};
	out[0]=(T)(P[0][0]*a[0]+P[1][0]*a[1]+P[2][0]*a[2]+P[3][0]*a[3]);
	out[1]=(T)(P[0][1]*a[0]+P[1][1]*a[1]+P[2][1]*a[2]+P[3][1]*a[3]);
	out[2]=(T)(P[0][2]*a[0]+P[1][2]*a[1]+P[2][2]*a[2]+P[3][2]*a[3]);
}
// Get Map Coordinates From Spheric Coordinates
// -PI/2<=lat<=PI/2 ; 0 < lon < 2*PI
template <typename T>
inline void sphereMap(const T P[3], T out[2])
{
	double lat,lon;
	lat=atan2(P[2],sqrt(P[0]*P[0]+P[1]*P[1]));
	lon=atan2(P[1],P[0]);
	out[MAP_U]=T(0.5*lon*INVPI);
	out[MAP_V]=T(lat*INVPI+0.5);
}
template <typename T>
inline void ellipsoidMap(double a, double b, const T P[3], T out[2])
{
	double lat,lon;
	lat=atan2(P[2]/b,sqrt(P[0]*P[0]+P[1]*P[1])/a);
	lon=atan2(P[1],P[0]);
	out[MAP_U]=T(0.5*lon*INVPI);
	out[MAP_V]=T(lat*INVPI);
}
template <typename T>
inline void ellipsoidMap(double e, const T P[3], T out[2])
{
	double lat,lon;
	lat=atan2(P[2],sqrt((1-e*e)*(P[0]*P[0]+P[1]*P[1])));
	lon=atan2(P[1],P[0]);
	out[MAP_U]=T(0.5*lon*INVPI);
	out[MAP_V]=T(lat*INVPI);
}

// Integration math
// Runge-Cutta Order Four
// Return Ending 1 Value By Runge-Cutta Order 4 Integration Of f
double RungeCutta4(	double xEnd, // Ending Arg Value
					double xStart, double yStart, // Starting Point2d: (xStart,yStart)
					int n, // Number Of Steps
					double (*f)(double, double), // Function Derivate: dy/dx=f(xEnd,y)
					double *data = NULL); // If Present, data Must Have At Least n+1 Points !!!!!!!
inline double RungeCutta4(
					double x, double y,
					double h, // Step Size
					double (*f)(double, double))
{
	double k1=h*f(x, y);
	double k2=h*f(x+h/2, y+k1/2);
	double k3=h*f(x+h/2, y+k2/2);
	double k4=h*f(x+h, y+k3);
	return y+(k1+2*(k2+k3)+k4)/6;
}
// DFT
BOOL getDFT(const double *data, double *dft, int count);
BOOL getDFT(const double *data, double *dft_Re, double *dft_Im, int count);
BOOL getIDFT(const double *dft_Re, const double *dft_Im, double *idft, int count);
// Auto Correlation Function Extraction
int getAutoCorrelFunc(	// Calculate The Auto Correlation In The Range 0:shift
						// Return The Used Scan Length Or 0 In Case Of Error
						const double *data, // data samples
						int count, // length of the data samples
						double *R, // auto correlation function data
						int shift, // max shift for auto-correlation calculation
						int index = 0, // start index for auto-correlation calculation
						int scan = -1); // scan length for auto-correlation calculation
// Filtering
int getFIROrder(	int windowType, // Type of the Truncation Window: FIR_WIN_TYPES
					double skirtLength, // Difference between passBand and stopBand
					double sampFreq, // Sample rate frequency
					BOOL forceOdd = TRUE); // Set to TRUE if only odd number should be produced
double getFIRSkirt(	int windowType, // Type of the Truncation Window: FIR_WIN_TYPES
					int order, // Filter order
					double sampFreq); // Sample rate frequency
BOOL getFIRFilter(	double *fir, // Filter coefficients
					int order, // Filter coefficients count, must be odd number
					int filterType, // Filter type: LOWPASS, HIGHPASS, BANDPASS, BANDSTOP
					int windowType, // Type of the Truncation Window: FIR_WIN_TYPES
					double sampFreq, // Sample rate frequency
					double cutOffFreq1, // CutOff Frequency = (passBand+stopBand)/2
					double cutOffFreq2 = 0.0); // Second cuttOff frequency in the case of BANDPASS and BANDSTOP, must be inferior to cutOffFreq1
// Normal Distribution Law
inline double normalDensity(	double x,
								double mean = 0.0,
								double stdev = 1.0)
{
	return exp(-0.5*pow2((x-mean)/stdev))/(SQRT2*SQRTPI*stdev);
}
double normalDistrib(double x, double precision = 1e-7);
inline double normalDistrib(double x, double mean, double stdev, double precision = 1e-7)
{// Normal Distribution
	return 0.5+normalDistrib((x-mean)/stdev,-mean/stdev,precision);
}
inline double normalDistrib2(double xDownLim, double xUpLim, double precision = 1e-7)
{ // Normal Distribution With 0 Mean And 1 Standart Deviation
	return normalDistrib(xUpLim,precision)-normalDistrib(xDownLim,precision);
}
inline double normalDistrib2(	double xDownLim,
								double xUpLim,
								double mean,
								double stdev,
								double precision = 1e-7)
{// Normal Distribution
	return normalDistrib2((xDownLim-mean)/stdev,(xUpLim-mean)/stdev,precision);
}
template <typename T> class Limit
{
protected:
	int count;
public:
	T minValue,maxValue;
	int minIndex,maxIndex;
	Limit(){clear();}
	void clear()
	{
		count=0;
		minValue=maxValue=0;
		minIndex=maxIndex=-1;
	}
	void copy(const Limit<T> &lim)
	{
		minValue=lim.minValue;
		maxValue=lim.maxValue;
		minIndex=lim.minIndex;
		maxIndex=lim.maxIndex;
		count=lim.count;
	}
	void set(const T& value)
	{
		minValue=maxValue=value;
		minIndex=maxIndex=0;
		count=1;
	}
	void set(const T& newMinVal, const T& newMaxVal)
	{
		minValue=newMinVal;
		maxValue=newMaxVal;
		minIndex=maxIndex=0;
		count=1;
	}
	void add(const T& newMinVal, const T& newMaxVal)
	{
		if(count)
		{
			if(minValue>newMinVal)
			{
				minIndex=count;
				minValue=newMinVal;
			}
			if(maxValue<newMaxVal)
			{
				maxIndex=count;
				maxValue=newMaxVal;
			}
		}
		else
		{
			minIndex=maxIndex=0;
			minValue=newMinVal;
			maxValue=newMaxVal;
		}
		count++;
	}
	void add(const T& value)
	{
		if(count)
		{
			if(minValue>value)
			{
				minIndex=count;
				minValue=value;
			}
			if(maxValue<value)
			{
				maxIndex=count;
				maxValue=value;
			}
		}
		else
		{
			minIndex=maxIndex=0;
			minValue=maxValue=value;
		}
		count++;
	}
	void add(const Limit<T> &lim){add(lim.minValue,lim.maxValue);}
	BOOL add(const T *values, int length, BOOL reset = FALSE)
	{
		if(!values||!length)
			return FALSE;
		if(!count||reset)
		{
			minValue=maxValue=values[0];
			minIndex=maxIndex=0;
			count=0;
		}
		for(int i=0;i<length;i++)
		{
			if(minValue>values[i])
			{
				minValue=values[i];
				minIndex=count;
			}
			else if(maxValue<values[i])
			{
				maxValue=values[i];
				maxIndex=count;
			}
			count++;
		}
		return TRUE;
	}
	T change(const T& dmin, const T& dmax)
	{
		minValue+=dmin;
		maxValue+=dmax;
		return maxValue-minValue;
	}
	T change(double k)
	{
		double newSpan=(maxValue-minValue)*(k-1);
		minValue-=0.5*newSpan;
		maxValue+=0.5*newSpan;
		return (T)newSpan;
	}
	BOOL check(const T& value) const {return (value>=minValue)&&(value<=maxValue);}
	double getRatio(const T& value) const {return (value-minValue)/(maxValue-minValue);}
	T span() const {return maxValue-minValue;}
};

// Analyse Math
//Least Squares Method
BOOL getLSMPolyCoef(// Get Polynomial Coefs By Using The Least Squares Method:
						 // SUM((Yi - SUM(Ck*Xi^k))^2)=min =>derivate for all Cj
						 //	i=0:N-1   k=0:M-1
						 // =>M equations:	SUM(Yi*Xi^j)=SUM(Ck * SUM(Xi^(k+j)))
						 //					i=0:N-1      k=0:M-1  i=0:N-1
						 // => C=inv(A)*B
						 double *coefs, // C
						 int count, // M
						 const double *y, // y data
						 const double *x, // x data
						 int length, // N
						 double x0 = 0.0, // x0
						 double y0 = 0.0, // x0
						 double eps = 0.0); // Matrix Singularity Factor


//Interpolation

// Get Interpolation Matrix:
 // y=SUM(Yi*MUL((X-Xj)/(Xi-Xj)))=SUM(Yi*SUM(Cik*(X-Xc)^k))
 //   i=0:N-1,j=0:N-1,j!=i			     k=0:N-1
BOOL getInterTable( MATRIX c, // Matrix (NxN) Cik 
					int n, // N
					const double *x = NULL, // X0,X1,...,Xn-1
					double xc = 0.0); // Interpolation Origin Xc
// Get Interpolation Coefficients:
// y=SUM(Yi*SUM(Cik*(0-Xc)^k))=SUM(Ak*(0-Xc)^k)
//   i=0:N-1,k=0:N-1
BOOL getInterCoef(	double *coef, // A0,A1,...,An-1
					int n,
					const double *y, // Y0,Y1,...,Yn-1
					CMATRIX c); // Interpolation Table (NxN) Cij); // N
// Get n Interpolation Coefs For y=f(x-x0)
BOOL getInterCoef( 	double *coef, // A0,A1,...,An-1
					int n,
					const double *y, // n Values For y
					const double *x = NULL, // n Values For x
					double x0 = 0.0);  // Interpolation Origin
// Get n Taylor Coefs For y=f(x-x0)
BOOL getTaylorCoef( double *coef, // A0,A1,...,An-1
					int n,
					double (*dydx)(double,int), // Derivate k Function
					double x0 = 0.0); 
// Calculate Function By Its Polynom Coefficients
double funcValue(	double x, // x Value
					const double *coef, // Polynom coefficients
					int n, // n Coefficients
					double x0 = 0.0, // Interpolation Origin
					double k = 1.0); // Multiplier k*(x-x0)
// Calculate Integral By The Polynom Coefficients Of The Function
double integValue(	double x, // x Value
					const double *coef, // Polynom coefficients
					int n, // n Coefficients
					double y0 = 0.0, // Integration origin
					double x0 = 0.0,
					double k = 1.0);
// Calculate Derivate By The Polynom Coefficients Of The Function
double derivValue( 	double x, // x Value
					const double *coef, // Polynom coefficients
					int n, // n Coefficients
					double x0 = 0.0,
					double k = 1.0);

template <typename T>
inline void incData(	T start, T end,
						int n, T * data,
						T (*f)(T) = NULL)
{
	if(!(n>0&&data))
		return;
	T step;
	if(n>1)
		step=(end-start)/(n-1);
	if(f)
	{
		data[0]=f(start);
		for(int i=1;i<n;i++)
			data[i]=f(start+i*step);
	}
	else
	{
		data[0]=start;
		for(int i=1;i<n;i++)
			data[i]=start+i*step;
	}
}
class Interpolator
{
protected:
	int interSize,dataSize,index;
	double stepSize,stepSizeRec,start,end,x0;
	Vector<double> interCoef, tmpBuffer;
	double *data;
	Table<double> interTable;
public:
	double value;
	Interpolator();
	int init(double *data, int dataSize, double start = 0.0, double end = 1.0, int interSize = 2);
	BOOL set(double x);
	int getInterSize()const{return interSize;}
	int getDataSize()const{return dataSize;}
	int getIndex()const{return index;}
};
class GaussMarkov
{
protected:
	double c1,c2;
public:
	double x;
	double stdev; // Standart Deviation
	double beta; // 1/(Time Constant)
	double dt;
	int realScan;
	Vector<double> buff_lnR,buff_t_lnR,buff_R,buff_t_R;
	GaussMarkov();
	GaussMarkov(double stdev, double beta, double sample);
	double rand();
	double next(){return (x=c1*x+c2*random_n());}
	double next(double sample);
	void init(double newStdev = 1.0, double newBeta = 1.0, double newSample = 1.0, BOOL random = TRUE);
	int init(	// Get The Parameters Of A Gauss-Markov Process From A Noise Signal,
				// By Using The Least Squares Method After Linearization Of The
				// Signal's Auto-Correlation Function Width Logarithms
				// Return The Used Scan Length Or 0 In Case Of Error
				const double *y, // y data samples
				const double *x, // x data samples
				int count, // length of the signal samples
				int shift, // max shift for auto-correlation calculation
				double dt = 1.0, // sample time to use if (x) doesn't exist and in the generator
				int index = 0, // start index for auto-correlation calculation
				int scan = 0); // scan length for auto-correlation calculation
	double autocorrel(double shift)const{return stdev*stdev*exp(-beta*ABS(shift));}
	static double getTime(	// Get The Requested Time For The Input 'data' So The Ratio Between
							// The Deviation Of The Autocorrelation And The Deviation Of The Process
							// Is Equal To 'ratio':
							double ratio, double beta){return (beta&&ratio)?(2.0/(beta*ratio*ratio)):0;}
	static int getSamples(	// Get The Requested Number Of Samples Of Length 'sample'
							// For The Input 'data' So The Ratio Between
							// The Deviation Of The Autocorrelation And The Deviation Of The Process
							// Is Equal To 'ratio':
							double ratio, double beta, double sample){return (beta&&ratio&&sample)?round(2.0/(beta*ratio*ratio*sample)):0;}
	double getTime(double ratio){return getTime(ratio,beta);}
	int getSamples(double ratio){return getSamples(ratio,beta,dt);}
};

}; //namespace Lio

#endif // __LIOMATH_H_